import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, GroupAction,
                            IncludeLaunchDescription, SetEnvironmentVariable)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node, PushRosNamespace
from launch_ros.descriptions import ParameterFile

'''
source ~/agst_slam_ros2/install/setup.bash
source ~/agst_robot_ros2/install/local_setup.bash
ros2 launch startup m50_startup.launch.py
'''


def generate_launch_description():
    # 定义机器类型
    robot_type = "trobot"

    # 配置目录、文件路径
    startup_dir = get_package_share_directory('trobot_startup')
    params_dir =  os.path.join(startup_dir, 'params')
    launch_dir =  os.path.join(startup_dir, 'launch')
    
    # 定义默认参数
    robot_type_arg = DeclareLaunchArgument(
        'robot_type', default_value = robot_type,
        description='Use simulation (Gazebo) clock if true')
    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value = 'false',
        description='Use simulation (Gazebo) clock if true')
    global_parameter_file_arg = DeclareLaunchArgument(
        'global_parameter_file', default_value = os.path.join(params_dir, 'global_params.yaml'),
        description='')
    voice_parameter_file_arg = DeclareLaunchArgument(
        'voice_parameter_file', default_value = os.path.join(params_dir, 'voice_params.yaml'),
        description='')
    default_system_parameter_file_arg = DeclareLaunchArgument(
        'default_system_parameter_file', default_value = os.path.join(params_dir, robot_type, 'default_system_params.yaml'),
        description='')
    default_robot_parameter_file_arg = DeclareLaunchArgument(
        'default_robot_parameter_file', default_value = os.path.join(params_dir, robot_type, 'default_robot_params.yaml'),
        description='')
    default_maps_parameter_file_arg = DeclareLaunchArgument(
        'default_maps_parameter_file', default_value = os.path.join(params_dir, robot_type, 'default_maps_params.yaml'),
        description='')
    default_nav_parameter_file_arg = DeclareLaunchArgument(
        'default_nav_parameter_file', default_value = os.path.join(params_dir, robot_type, 'default_nav_params.yaml'),
        description='')

    # 定义启动组
    bringup_group = GroupAction([
        # 启动底盘模块
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(launch_dir, robot_type, "{}_base.launch.py".format(robot_type))),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                              'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                              'default_system_parameter_file': LaunchConfiguration('default_system_parameter_file'),
                              'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                              }.items()),
        # 启动系统功能模块
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(launch_dir, robot_type, "{}_system.launch.py".format(robot_type))),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                              'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                              'voice_parameter_file': LaunchConfiguration('voice_parameter_file'),
                              'default_system_parameter_file': LaunchConfiguration('default_system_parameter_file'),
                              }.items()),
        # 启动感知、地图功能模块
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(launch_dir, robot_type, "{}_map.launch.py".format(robot_type))),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                              'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                              'default_system_parameter_file': LaunchConfiguration('default_system_parameter_file'),
                              'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                              'default_maps_parameter_file': LaunchConfiguration('default_maps_parameter_file'),
                              }.items()),
                              
        # 启动路径规划、任务、规控模块
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(launch_dir, robot_type, "{}_nav.launch.py".format(robot_type))),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                              'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                              'default_system_parameter_file': LaunchConfiguration('default_system_parameter_file'),
                              'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                              'default_maps_parameter_file': LaunchConfiguration('default_maps_parameter_file'),
                              'default_nav_parameter_file': LaunchConfiguration('default_nav_parameter_file'),
                              }.items()),
    ])

    ld = LaunchDescription()
    ld.add_action(robot_type_arg)
    ld.add_action(use_sim_time_arg)
    ld.add_action(global_parameter_file_arg)
    ld.add_action(voice_parameter_file_arg)
    ld.add_action(default_system_parameter_file_arg)
    ld.add_action(default_robot_parameter_file_arg)
    ld.add_action(default_maps_parameter_file_arg)
    ld.add_action(default_nav_parameter_file_arg)
    ld.add_action(bringup_group)

    return ld